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Abstract: Accurate estimation of the motion and shape of a moving object is a challenging 
task due to great variety of noises present from sources such as electronic components and 
the influence of the external environment, etc. To alleviate the noise, the filtering/ 
estimation approach can be used to reduce it in streaming video to obtain better estimation 
accuracy in feature points on the moving objects. To deal with the filtering problem in the 
appropriate nonlinear system, the extended Kalman filter (EKF), which neglects 
higher-order derivatives in the linearization process, has been very popular. The unscented 
Kalman filter (UKF), which uses a deterministic sampling approach to capture the mean and 
covariance estimates with a minimal set of sample points, is able to achieve at least the 
second order accuracy without Jacobians' computation involved. In this paper, the UKF is 
applied to the rigid body motion and shape dynamics to estimate feature points on moving 
objects. The performance evaluation is carried out through the numerical study. The results 
show that UKF demonstrates substantial improvement in accuracy estimation for 
implementing the estimation of motion and planar surface parameters of a single camera. 
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1. Introduction 

The problem of estimating positions and velocities of moving features in space leads to the problem 
of estimating motion and shape parameters of moving features from their corresponding image data 
observed over time. This is important in various engineering applications, such as robotics and machine 
vision [1-6]. A dynamical systems approach to machine vision is introduced in [1], in which the problem 
of motion and shape parameter estimation is described as an inverse problem associated with a pair of 
coupled Riccati partial differential equations. Identification of the motion and shape parameters using 
the estimation technique was discussed in [5,6]. A CCD camera with a laser range finder mounted on a 
mobile robot for better motion and shape parameters identification can be seen 
in [5]. A sliding mode approach was proposed to estimate the motion of a moving body with the aid of 
a CCD camera [6]. For performance improvement, various estimation techniques have been adopted to 
reduce the noise. The algebraic methods yielded feasible results, but they were computationally 
unrealistic. The Kalman filter method is used in the estimation of the motion parameters to reduce the effect 
of the measurement errors which are inevitable in the real world. The Kalman filter (KF) method [7] for 
estimation of the 3D camera motion in imagine sequences for the applications to the video coding 
system is proposed by Kim et al. [8]. Kano et al. [9] performed a numerical study and compared an 
extended Kalman filter (EKF) based recursive algorithm with a non-recursive algebraic method for 
estimating motion and planar surface parameters. 

The KF is theoretically attractive because it has been shown to be the one that minimizes the 
variance of the estimation mean square error (MSE). The nonlinear filter is used for nonlinear dynamics 
and/or nonlinear measurement relationships. The problem of estimating the state variables of the 
nonlinear systems may be solved using the nonlinear version of the Kalman filter. The most popular 
form is the EKF. The fact that EKF highly depends on a predefined dynamics model forms a major 
drawback. To achieve good filtering results, the designers are required to have the complete 
a priori knowledge on both the dynamic process and measurement models, in addition to the 
assumption that both the process and measurement are corrupted by zero-mean Gaussian white 
sequences. If the input data does not reflect the real model, the estimates may not be reliable. 

Similar to the EKF, the unscented Kalman filter (UKF) [10-17] focuses on approximating the 
prediction probability characteristics and use the standard minimum mean square error estimator. The 
UKF has been developed in the context of state estimation of dynamic systems as a nonlinear 
distribution (or densities in the continuous case) approximation method. The UKF is superior to EKF 
not only in theory but also in many practical situations. The algorithm performs the prediction of the 
statistics with a set of carefully chosen sample points for capturing mean and covariance of the system. 
These sample points are sometimes referred to as the sigma points employed to propagate the 
probability of state distribution. The basic premise behind the UKF is it is easier to approximate a 
Gaussian distribution than it is to approximate an arbitrary nonlinear function. Instead of linearizing 
using Jacobian matrices as in the EKF and achieving first-order accuracy, the UKF can capture the 
states up to at least second order by using a deterministic sampling approach to capture the mean and 
covariance estimates with a minimal set of sample points. The deterministic sampling based UKF with 
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applications on estimation of rigid body motion and shape dynamics are presented to estimate the 
feature points on the moving object. Results obtained shows that UKF is able to provide more accurate 
and reliable estimation accuracy of the object. Investigation of the UKF approach to the motion and 
shape estimation problem has not been seen in the literature. 

This paper is organized as follows. In Section 2, preliminary background on rigid body motion is 
reviewed. The shape dynamics and optical flow dynamics are discussed in Sections 3 and 4, 
respectively. The unscented Kalman filter is introduced in Section 5. Results and Discussion are given in 
Section 6. Conclusions are given in Section 7. 



2. Rigid Body Motion 

The mathematical description of a 3D point undergoing a rigid transformation about the camera axes 
is given as follows. Let co x , co y and co z represent the angle of rotation about the X, Y and Z axes, 
respectively. Figure 1 illustrates the world coordinate system. An arbitrary rotation R can be 
represented by successive rotations around theX, 7 and Z axes, respectively, as: 



R 



1 0 

0 cos(co x ) 
0 sin(&> x ) 



0 

-sin(> x ) 
cosO x ) 



cos(&>y ) 0 sin(&>y ) 

0 10 
- sin(&>y ) 0 cos(&>y ) 



cos(&> z ) - sin(&> z ) 0 
sin(&> z ) cos(co z ) 0 
0 0 1 



(1) 



Assuming infinitesimal rotations, the zeroth order terms of the Taylor series expansion of the 
trigonometric functions sin and cos provide the following approximations: 

cos(#)*l, sin(#)*# (2) 

Using the approximations in Equation (2), R can be approximated in the skew- symmetric matrix 
form in terms of angular velocity: 
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Figure 1. Illustration of the world coordinate system [3]. 
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Let the vector T = [t x t y t z ] represent the translational velocity, where the elemental components t x , 
t and t z represent the translational velocities in the X, Y and Z directions, respectively. The velocity 
vector V = [XY Z] of a point in the world coordinates F = [XYZ] with respect to camera 
coordinates undergoing rigid transformation is represented as: 
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-03 z 


COy 


~x~ 
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ty 


(4) 
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which may be written in matrix form: 



V = (R-I)P + T 



where I represents a 3 x 3 identity matrix. By defining [-co z co -a> x ] as [co x a> 2 a> 3 ] ; and 



[tx l y t z ] T as [b x b 2 b 3 ] T , Equation (4) can be represented as: 



V=QP+b 



where b = [b l b 2 b 3 ] T and: 



0 co } oj 2 
-coy 0 a> 3 
-a>2 -ty 0 



(5) 



(6) 



Consider the dynamical system Equation (5) where it is assumed that [co x co 2 a> 3 ] T * 0 . If: 

b g ImS2 

it is easily seen that Im£2 is a plane in 9? given as: 



where: 



3. Shape Dynamics 



ImQ = {xG^|co i x = 0} 

T 

CO =[#>3 - OJ 2 CO\\ 



(7) 

(8) 
(9) 



The motion field describing the motion of individual points on the surface might undergo a change in 
shape. The dynamical system which describes the changing shape of the surface is called the shape 
dynamics. Let (X, 7, Z) be the world coordinate frame wherein we have a surface defined by: 

Z = S(X 9 Y,t) (10) 

Assume that S is smooth enough so that its derivatives with respect to each of the variables are 
defined everywhere. The motion field is assumed to be described by: 

X = f{X,Y,Z), Y = g{x,Y,z), Z = h{x,Y,z) (11) 

How the surface as in Equation (10) moves as points on the surface move following the motion field 
as in Equation (11) can be described by the quasi- linear partial differential equation called the 
"shape dynamics": 
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Consider the initial condition: 



^ + f{X, Y, Z)^- + S {X, Y, Z)^ = A(jr, 7, Z) 



S(X,F,0) = ^(X,Y) 



(12) 



(13) 



The pair of Equations (12) and (13) constitutes an example of a Riccati partial differential equation. 
The surface defined by Equation (10) is assumed to be a plane described by: 

Z = pX + qY + r (14) 

where p, q, r are shape parameters that are time varying as a result of the motion field. The vectors 
(X, 7, Z) and (p,q,r) are written in terms of homogeneous coordinates (x Y Z w): 



and: 
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where W = (X 1 + Y 2 + Z 2 ) 1 ' 2 . Equation (14) can then be rewritten as: 

pX + qf -sZ + rW =0 
Using Equation (15), Equation (5) can be written as: 
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The shape dynamics can be obtained by differentiating Equation (17) with respect to time t: 
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(15) 
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(18) 



(19) 



where p(t) , q(i) and r(t) are the shape parameters to be discussed in Section 4. 



4. Optical Flow Dynamics 

As shown in Figure 2, the 3D vector P = (X,7,Z) is assumed to be observed via perspective 
projection onto a plane parallel to the (x, y) axes and located at Z = 1 by defining the relationship 
between an image point u = (x, y) and a scene point (X, 7, Z) given by: 
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Figure 2. The relationship between an image point u = (x 9 y) and a scene point P = (X,7,Z) . 
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P=(X,Y,Z) 




The focal length / = 1 is usually used without loss of generality. With the relation given by Equation 
(5), differentiating Equation (20) leads to the relations: 



ZX — XZ X Z ( \ / \ th bn. 

x = = x — = \0J\ y + (D2)- x[- 0)2 X ~ <^3 > ; J + — ~x — 



(21) 



and: 



ZY-YZ Y 



From Equations (14) and (20), we have: 



1 _ 1 - px - qy 
Z~ r 



and hence the optical flow equations are given by: 



x = a> 2 + co x y + co 2 x 2 + co 3 xy + (q - xc 3 )(1 - px - qy) 
y = a> 3 - co x x + co 2 xy + a> 3 y 2 + (c 2 - jc 3 )(1 - px- qy) 



(22) 

(23) 

(24) 
(25) 



where c^bjr , i = 1,2,3. 

Equations (24) and (25) denote the optical flow equations. Even when the motion field is time 
invariant, the parameters of the optical flow equations could be time-varying due to the fact that the 
shape parameters are changing in time as a result of motion field described by Equation (11). Asf— »oo, 
we have: 



CO 



CO, 



2 r(f)->— c f ->-^, i = l,2,3 



■J^ CO\ U/] 

In particular, if belmQ, the shape parameters p(t), q(t) and are periodic functions satisfying 
the Riccati Equation [2,9]: 



CO, 



bt 



(26) 
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p = -a> 2 (1 + p 2 ) + co { q - co 3 pq 
q = -co 3 (l + q 2 )- (D x p - 0) 2 pq 
r = b 3 - b x p - b 2 q - r(co 3 q + co 2 p) 



(27) 



which is parameterized by a total of six motion parameters and three initial conditions on shape 
parameters. Therefore, a total of nine parameters need to be adopted for describing the shape dynamics. 
The Riccati equation propagates in time the relationship between coordinates X, Y, and Z expressed via 
the surface described by Equation (10). 

Once the modeling of shape dynamics and optical flow dynamics is accomplished, the estimation 
algorithm can be employed for implementing the estimation of motion and planar surface parameters. In 
addition to the algebraic methods, the recursive estimation algorithms are applicable. The recursive 
algorithm presented by Kano et al. [9] is an EKF-based algorithm. The motivation of the paper is to 
carry out the UKF-based approach, which has not been seen in the literature for the motion and shape 
estimation problem. A brief introduction of the UKF is provided in Section 5. 

5. The Unscented Kalman Filter 

The UKF is a nonlinear filter which deals with the case governed by the nonlinear stochastic 
difference equations: 



where the state vector x^ e9T, process noise vector e9i w , measurement vector z k e9i m , and 
measurement noise vector e 9i m . In Equation (28), both the vectors and are zero mean 
Gaussian white sequence having zero crosscorrelation with each other: 



where is the process noise covariance matrix, R k is the measurement noise covariance matrix. 

5.1. The Unscented Transformation 

The first step in the UKF is to sample the prior state distribution, i.e., generate the sigma points 
through the unscented transformation (UT). Figure 3 illustrates the true means and co variances as 
compared to those obtained by the mapping of the UKF versus that of the EKF. The dot-line ellipse 
represents the true covariance. The UKF is implemented through the transformation of the nonlinear 
function f (•) , shown as the solid-line ellipse on the top portion of the figure; the EKF is accomplished 
through the Jacobian F = df/dx, shown as the solid- line ellipse at the bottom portion of the figure. The 
UKF approach estimates are expected to be closer to the true values than the EKF approach. 

Several UT's are available. One of the popular approaches is the scaled unscented 
transformation [15-17]. Consider an n dimensional random variable x , having the mean x and 
covariance P , and suppose that it propagates through an arbitrary nonlinear function f . The unscented 
transform creates 2n + l sigma vectors X (a capital letter) and weighted points, given by: 



(28a) 



(28b) 




(29) 
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X (0) 



X (0 = x + Q(n + k)Y) T , , /' = 



(30a) 
(30b) 

(30c) 



X (i+n) = x-(V(n + 2)P)f, i = l,...,n 

where (^(m + A)?),- is the i th row of the matrix square root. ^/(« + 1)P can be obtained from the 
lower- triangular matrix of the Cholesky factorization; X = a 2 {n + K)-n is a scaling parameter; 
a determines the spread of the sigma points around x and is usually set to a small positive 
(e.g., le-4 < a < 1); k is a secondly scaling parameter (usually set as 0); /? is used to incorporate prior 
knowledge of the distribution of x (when x is normally distributed, /? = 2 is an optimal value); 

is the weight for the mean associated with the rth point; and W- c) is the weigh for the covariance 
associated with the ith point: 



W< m) = — 



(n + X) 

Wjf } = W$ n) + (l-a 2 +P) 



W jm) =w jc) = J_ 

' ' 2(n + X) 
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The sigma vectors are propagated through the nonlinear function to yield a set of transformed 
sigma points: 

y,=/(X (,) ), i = 0,...,2n (32) 

The mean and covariance of y . are approximated by a weighted average mean and covariance of the 
transformed sigma points as follows: 



2n 
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In 



P« = Z»/ (c) (y/-y«Xy/-y„) J 
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(33) 
(34) 



Figure 3. Illustration of properties of UKF and EKF, Reproduced with permission from Yong Li [14]. 
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5.2. The Unscented Kalman Filter 



A high level of operation of the unscented Kalman filter is shown in Figure 4. To look at the detailed 
algorithm of the UKF, firstly, the set of sigma points are created by Equation (30). After the sigma 
points are generated, the time update (prediction step) of the UKF involves the following steps: 

G;W(X«)> i = 0,...,2#i 



i=o 



In 



(Z^),.=h((^),.) 



/=0 



(35) 
(36) 

(37) 
(38) 
(39) 



Figure 4. High level of operation of the unscented Kalman filter. 
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The measurement update (correction) step of the UKF involves the following steps: 



K =2Xte,. -z,][(Z,) ; . -i k f + R k (40) 

i=0 
In 

K = £*fto -z^f (41) 

K^P^P; 1 (42) 
^=^+K,(z,-z-) (43) 

P,=P,--K,P ZZ K[ (44) 

The samples are propagated through true nonlinear equations, which can capture the states up to at 
least second order. 

6. Results and Discussion 

Simulation experiments have been carried out to evaluate the performance of the UKF approach in 
comparison with the conventional EKF approach for estimating motion and shape parameters. 
Computer code was developed using MATLAB. It is assumed that, at each time instant k , a set of 
three feature points {x i k , y ik ) , i = 1, 2, 3 are observed. The state vector x k is given by: 

x k=k,* ^2,* (°xk c u c i,k c 3,k Pk (ik x i,k y\,k x 2, k y 2 , k x xk y^Y ( 45 ) 

The associated state equation in discretized form is given by: 

x k+1 =/(x k ) (46) 
which is constituted from the set of Equations (24,25) and (27), as follows: 

PM = (°>l,k<lk ~ °>2,k ~ ®2,kPk ~ &3,kPk1k ) At + Pk 

= (-o>ukPk - ®u - (°2,kPk(lk ~ <m\ ) At + Ik (47) 

^+1 = ( b 3,k ~\kPk ~ h 2,k1k - r k{(°Xk<ik +i0 2,kPk)) At + r k 



x k + \ = (®2,t + ^i, k y k + «>2,k x2 k + ^ Xk x k y k + (c u - x k c u )(\ - p k x k - q k y k ))At + x k 



(48) 



y k+ \ = («*»,* - °>\,k x k + ®2,k x kyk + °>3,kyk + - y^ )0 - Pk x k - q k y k )) At + y k 

c uk =hklr k (49) 
along with the random walk models for the angular velocities co t k+l = co i kx , i = l,2,3 . Let 
z k e 91 6 represent the observation vector, the observation equation can then be written as: 

z k =Ux k+ y k (50) 

where \ k is assumed to be a zero mean Gaussian white sequences noise with co variance 
R = <t 2 I 6 ( a = 0.01 ). The elements of the measurement model H 2nxl4 is defined by: 

H = [0 2 „ x8 I 2B ],» = 3 
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The process noise covariance matrix is given by Q = 10" 10 xl 14 and the parameters utilized in the 
UKF are given as follows: a = le-^ , (3 = 2 ,k = 0 . The sigma points capture the same mean and 
covariance irrespective of the choice of matrix square root which is used. The numerical efficient and 
stable method such as the Cholesky factorization has been used in obtaining the sigma points. 

Experiment was conducted on a simulated three feature points: (2.5,2.5), (1.5,0.5) and (1.0,1.5) 
locations. The motion and shape parameters Q is set as (q)\ 9 co 2 , CO3) = (~0.2, 0.1, -0.1), with initial 
values for shape parameters are (p 0 , qo, ^0) = (0.1, 0.1, 2.0). For comparative purposes, the following 
two cases are considered [2]. 

Casel: b = (0.1 0.1 -O.lfelmQ 

Case2: b = (0.1 0.1 0.1) r g ImQ 

Figures 5-12 show the results for the numerical experiments. Figure 5 shows the sample trajectories 
for the three feature points on image plane for the case b e ImQ. It can be seen that the motions are 
circular and periodic around the axis of rotation. Figures 6-8 are the results for the case when b e ImQ . 
Estimation results for parameters c x , c 2 and c 3 for the three feature points are given in Figure 6. Three 
curves, shown in red, green, and black colors, denote the true, EKF-based, and 
UKF-based estimated values, respectively. For better clarity, the estimation errors for the three feature 
points are shown in Figure 7, which illustrates the advantage of UKF. Figure 8 gives the estimation 
accuracy for the shape parameters p k , q k and r k . The EKF is not working very well whereas the UKF 
demonstrates its good capability for capturing the true trajectories. Comparison of RMSE (in the 
unit of mm) of the three feature points on the image plane using the EKF and UKF is summarized in 
Table 1. Comparison of RMSE (in the unit of mm) for the feature points of the scene point using the 
EKF and UKF is summarized in Table 2. 

Figure 5. Sample trajectories for the three feature points on image plane for the case b <e ImQ . 
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Figure 6. Estimation results for parameters c x , c 2 and c 3 for the three feature points for the 
case b e ImQ (true-in red; EKF-in green; UKF-in black). 
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Figure 7. Estimation errors for the three feature points for the case b e ImQ 
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Figure 8. Comparison of estimation accuracy for shape parameters p k , q k and r k , for the 
case belmQ (true-in red; EKF- in green; UKF-in black). 
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Table 1. Comparison of RMSE (in the unit of mm) of the three feature points on the image 
plane using the EKF and UKF ( b e ImQ ). 
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Table 2. Comparison of RMSE (in the unit of mm) for the feature points of the scene point 
using the EKF and UKF ( b e ImQ ). 

EKF UKF 

X Y Z X-Y-Z X Y Z X-Y-Z 

Feature point 1 (xl.yl) 5.35 5.6286 5.4815 9.5053 1.5783 1.7815 0.7082 2.4832 
Feature point 2 (x2,y2) 2.4956 1.0329 1.7686 3.2284 0.621 0.2423 0.3968 0.7757 
Feature point 3 (x3,y3) 1.6892 2.546 1.9205 3.6088 0.5264 0.8431 0.5515 1.1367 
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Figure 9 shows the sample trajectories for the three feature points on image plane for the case 
b g ImQ . The performance comparison for EKF and UKF in the case of b g ImQ is basically similar to 
the results obtained for b e ImQ. The motions are receding to infinitely linearly in time. Figures 10-12 
provide the results for the case when b £ ImQ. Figure 10 shows the estimation results for parameters 
q , c 2 and c 3 for the three feature points; Figure 1 1 provides the estimation errors for the three feature 
points; and Figure 12 gives the estimation accuracy for the shape parameters p k , q k and r k . 
Comparison of RMSE (in the unit of mm) of the three feature points on the image plane using the EKF 
and UKF is summarized in Table 3. Comparison of RMSE (in the unit of mm) for the feature points of 
the scene point using the EKF and UKF is summarized in Table 4. 

Figure 9. Sample trajectories for the three feature points on image plane for the case b g ImQ . 
3. . . . . . 1 i — (xi,y0 
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Figure 10. Comparison of estimation results for parameters c u c 2 , and c 3 for the three 
feature points for the case b <£ ImQ (true-in red; EKF- in green; UKF-in black). 
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Figure 12. Comparison of estimation accuracy for shape parameters p k , q k and r k , for the 
case b € ImQ (true-in red; EKF- in green; UKF-in black). 
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Figure 12. Cont. 
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Table 3. Comparison of RMSE (in the unit of mm) of the three feature points on the image 
plane using the EKF and UKF ( b g ImQ ). 

EKF UKF 

x y x-y x y x-y 

Feature point 1 (xl,yl) 0.0056 0.0073 0.0092 0.001 0.0011 0.0015 
Feature point 2 (x2,y2) 0.0033 0.0029 0.0044 0.0009 0.0008 0.001 1 
Feature point 3 (x3,y3) 0.0028 0.0025 0.0037 0.0008 0.001 0.0013 

Table 4. Comparison of RMSE (in the unit of mm) for the feature points of the scene point 
using the EKF and UKF (b g ImQ). 

EKF UKF 

X Y Z X-Y-Z X Y Z X-Y-Z 

Feature point 1 (xl,yl) 2.9116 2.557 5.1281 6.4276 0.476 1.4102 0.7018 1.6455 
Feature point 2 (x2,y2) 0.8623 1.0275 1.6439 2.1217 0.0938 0.0894 0.1173 0.1748 
Feature point 3 (x3,y3) 0.8424 1.1548 1.9177 2.3918 0.0562 0.087 0.1245 0.1619 

In general, utilization of an adequate nonlinear model with suitable filter makes it possible to achieve 
improved estimation performance. For the problem of single camera based motion and shape estimation, 
the UKF-based recursive estimation algorithm is a good alternative adopted for implementing the 
estimation of motion and planar surface parameters. The states and the dynamic process are related 
nonlinearly. The nonlinearity/uncertainty in the state estimate is suitably taken care of in the UKF, which 
has therefore demonstrated substantial state estimation accuracy improvement as compared to the EKF 
based approach. When compared with EKF, the UKF method exhibits superior performance since the 
series approximations in the EKF algorithm can lead to poor representations of the nonlinear functions 
and probability distributions of interest. 
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7. Conclusions 

Various engineering applications, such as robotics and machine vision, require the estimation of 
positions and velocities of moving features in space, leading to the problem of estimating motion and 
shape parameters of moving features from their corresponding image data observed over time. To 
alleviate the noise and obtain better estimation accuracy in feature points on the moving objects, the 
filtering/estimation approach can be used. A variety of estimation techniques have been adopted to 
reduce the noise. One of the most important ones is the extended Kalman filter (EKF). 

As compared to the EKF's linear approximation, the unscented transformation is accurate to the 
second order for any nonlinear function. In light of unscented Kalman filter's superiority over the 
extended Kalman filter, this paper has presented a deterministic sampling of UKF approach for 
estimating motion and shape parameters of feature points on the moving object. Such a motion is 
described by the nonlinear model with skew symmetric matrix Q , which is widely used in the theory of 
machine vision. The reason is due to the fact that the UKF is able to deal with the nonlinear formulation, 
which will ensure better accurate parameter estimation. For the nonlinear estimation problem, 
alternatives for the classical model-based extended Kalman filter (EKF) can be employed. The UKF is a 
nonlinear distribution approximation method, which uses a finite number of sigma points to propagate 
the probability of state distribution through the nonlinear dynamics of system. The UKF exhibits 
superior performance when compared with conventional EKF since the series approximations in the 
EKF algorithm can lead to poor representations of the nonlinear functions and probability distributions 
of interest. 

The analyses were confirmed by simulation studies. For both cases, b e ImQ and/? £ ImQ , motion 
and shape parameters were recovered successfully with remarkable accuracy improvement. The results 
obtained shows that the proposed UKF method have been compared to the EKF and have demonstrated 
substantial improvement in obtaining the motion and shape of moving objects. 
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